
#include "protocol.h"


/********************************************************************************************************/
/* 命令接收缓存 */
uint8_t RxBuffer[PTO_MAX_BUF_LEN];
/* 接收数据下标 */
uint8_t RxIndex = 0;
/* 接收状态机 */
uint8_t RxFlag = 0;
/* 新命令接收标志 */
uint8_t New_CMD_flag = 0;
/* 新命令数据长度 */
uint8_t New_CMD_length;
/********************************************************************************************************/
// 获取命令标志
uint8_t Get_CMD_Flag(void)
{
    return New_CMD_flag;
}
// 获取接收的数据
uint8_t *Get_RxBuffer(void)
{
    return (uint8_t *)RxBuffer;
}
// 获取命令长度
uint8_t Get_CMD_Length(void)
{
    return New_CMD_length;
}
// 清除命令数据和相关标志
void Clear_CMD_Flag(void)
{
    for (uint8_t i = 0; i < New_CMD_length; i++)
    {
        RxBuffer[i] = 0;
    }
    New_CMD_length = 0;
    New_CMD_flag = 0;
}
void PrintPacket(uint8_t *data, uint16_t len) {
    uint16_t i;
    printf("Packet Data (len=%d): ", len);
    for (i = 0; i < len; i++) {
        printf("0x%02X ", data[i]);
    }
    printf("\n");
}
/********************************************************************************************************/
// 接收数据
void Upper_Data_Receive(uint8_t Rx_Temp)
{
    if (New_CMD_flag != 0)
        return;
    switch (RxFlag)
    {
    case 0:
        if (Rx_Temp == PTO_HEAD)
        {
            RxBuffer[0] = PTO_HEAD;
            // printf("PTO_HEAD:%#2x   ", RxBuffer[0]);
            RxFlag = 1;
        }
        else
        {
            RxBuffer[0] = Rx_Temp;
            // printf("PTO_HEAD ERR:%#2x\n", RxBuffer[0]);
            RxBuffer[0] = 0x0;
            RxFlag = 0;
        }
        break;

    case 1:
        if (Rx_Temp == PTO_DEVICE_ID)
        {
            RxBuffer[1] = PTO_DEVICE_ID;
            // printf("PTO_DEVICE_ID:%#2x  ", RxBuffer[1]);
            RxFlag = 2;
            RxIndex = 2;
        }
        else
        {
            RxBuffer[1] = Rx_Temp;
            // printf("PTO_DEVICE_ID ERR:%#2x\n", RxBuffer[1]);
            RxFlag = 0;
            RxBuffer[0] = 0x0;
        }
        break;
    case 2:
        New_CMD_length = Rx_Temp + 2;
        if (New_CMD_length >= PTO_MAX_BUF_LEN)
        {
            // printf("PTO_MAX_BUF_LEN ERR:%#2x\n", New_CMD_length);
            RxIndex = 0;
            RxFlag = 0;
            RxBuffer[0] = 0;
            RxBuffer[1] = 0;
            New_CMD_length = 0;
            break;
        }
        RxBuffer[RxIndex] = Rx_Temp;
        // printf("PTO_MAX_BUF_LEN:%#2x",RxBuffer[RxIndex]);
        RxIndex++;
        RxFlag = 3;
        break;

    case 3:
        RxBuffer[RxIndex] = Rx_Temp;
        // printf("data:%#2x",RxBuffer[RxIndex]);
        RxIndex++;
        if (RxIndex >= New_CMD_length)
        {
            // printf("\n");
            New_CMD_flag = 1;
            RxIndex = 0;
            RxFlag = 0;
        }
        break;

    default:
        break;
    }
}

/**
 * 计算数据包的校验和（累加和）
 * @param data 数据包指针
 * @param len  数据长度（不包含校验和字节）
 * @return 校验和（8位无符号整数）
 */
uint8_t CalculateChecksum(uint8_t *data, uint16_t len) {
    uint16_t sum = 0;
    uint16_t i;
    
    // 累加所有数据字节
    for (i = 0; i < len; i++) {
        sum += data[i];
    }
    
    // 返回低8位作为校验和
    return (uint8_t)(sum & 0xFF);
}

/********************************************************************************************************/
// 解析数据
void Upper_Data_Parse(uint8_t *data_buf, uint8_t num)
{
    // 包头1 包头2 数量 功能 参数x 校验和
    /* 首先计算校验累加和 */
    int sum = 0;
    for (uint8_t i = 2; i < (num - 1); i++)
        sum += *(data_buf + i);
    sum = sum & 0xFF;
    /* 判断校验累加和 若不同则舍弃*/
    uint8_t recvSum = *(data_buf + num - 1);
    if (!(sum == recvSum))
    {
        printf("Check sum error!, CalSum:%d, recvSum:%d\n", sum, recvSum);
        for (uint8_t i = 0; i < num; i++)
        {
            printf("data_buf[%d]:%#2x    ", i, data_buf[i]);
        }
        printf("\n");
        return;
    }

    uint8_t func_id = *(data_buf + 3);

    switch (func_id)
    {
    /* 判断功能字：蜂鸣器控制 */
    case FUNC_BEEP:
    {
        uint16_t time = *(data_buf + 5) << 8 | *(data_buf + 4);
        printf("beep:%d\n", time);
        bsp_beep_play(time);
        break;
    }
    case FUNC_MOTOR:
    {
        int16_t speed[4] = {0};
        int8_t motor_1 = *(data_buf + 4);
        int8_t motor_2 = *(data_buf + 5);
        int8_t motor_3 = *(data_buf + 6);
        int8_t motor_4 = *(data_buf + 7);

        // printf("motor=%d, %d, %d, %d", motor_1, motor_2, motor_3, motor_4);

        int16_t motor_pulse = MOTOR_MAX_PULSE - MOTOR_IGNORE_PULSE;
        speed[0] = (int16_t)motor_1 * (motor_pulse / 100.0);
        speed[1] = (int16_t)motor_2 * (motor_pulse / 100.0);
        speed[2] = (int16_t)motor_3 * (motor_pulse / 100.0);
        speed[3] = (int16_t)motor_4 * (motor_pulse / 100.0);
        // PWM控制小车运动
        Motion_Set_Pwm(speed[0], speed[1], speed[2], speed[3]);
        break;
    }
    /* 控制小车运动 */
    case FUNC_CAR_RUN:
    {
        uint8_t parm = *(data_buf + 4);
        uint8_t state = *(data_buf + 5);
        uint16_t speed = *(data_buf + 7) << 8 | *(data_buf + 6);
        // printf("car_run=0x%02X, %d, %d", parm, state, speed);
        uint8_t adjust = parm & 0x80;
        Motion_Ctrl_State(state, speed, (adjust == 0 ? 0 : 1));
        break;
    }

    /* 判断功能字：小车速度设置 */
    case FUNC_MOTION:
    {
        uint8_t parm = (uint8_t)*(data_buf + 4);
        int16_t Vx_recv = *(data_buf + 6) << 8 | *(data_buf + 5);
        int16_t Vy_recv = *(data_buf + 8) << 8 | *(data_buf + 7);
        int16_t Vz_recv = *(data_buf + 10) << 8 | *(data_buf + 9);
        uint8_t adjust = parm & 0x80;
        // printf("motion: 0x%02X, %d, %d, %d\n", parm, Vx_recv, Vy_recv, Vz_recv);

        if (Vx_recv == 0 && Vy_recv == 0 && Vz_recv == 0)
        {
            Motion_Stop(STOP_BRAKE);
        }
        else
        {
            Motion_Ctrl(Vx_recv, Vy_recv, Vz_recv, (adjust == 0 ? 0 : 1));
        }
        break;
    }
    case FUN_CAR_TRACE:
    {
        printf("trace");
        uint8_t ctrl_flag = *(data_buf + 4);
            
            if (ctrl_flag == 1)
            {
                Trace_Ctrl(1);  // 开启循迹
                printf("Trace enabled\n");
            }
            else if (ctrl_flag == 0)
            {
                Trace_Ctrl(0);  // 关闭循迹
                Mecanum_State(0, 0, 0);  // 关闭循迹时停止电机（可选）
                printf("Trace disabled\n");
            }
            else
            {
                printf("Invalid trace command: %d\n", ctrl_flag);  // 无效指令提示
            }
            break;
    }
    case FUNC_SET_MOTOR_PID:
    {
        // 确保数据缓冲区长度足够
        
        
        // 解析PID参数（注意：已放大1000倍，实际值需除以1000）
        uint16_t p_value = (*(data_buf + 5) << 8) | *(data_buf + 4);
        uint16_t i_value = (*(data_buf + 7) << 8) | *(data_buf + 6);
        uint16_t d_value = (*(data_buf + 9) << 8) | *(data_buf + 8);
        uint8_t save_flag = *(data_buf + 10);
        
        // 转换为实际值（除以1000）
        float Kp = (float)p_value / 1000.0f;
        float Ki = (float)i_value / 1000.0f;
        float Kd = (float)d_value / 1000.0f;
        
        // 验证参数范围
        if (p_value > 10000 || i_value > 10000 || d_value > 10000) {
            printf("Error: PID parameters out of range (0-10000)\n");
            break;
        }
        
        // 设置速度PID参数
        PID_Set_Motor_Parm(4, Kp, Ki, Kd);
        
        // 如果需要永久保存参数
        // if (save_flag == 0x5F) {
        //     Save_Speed_PID_To_Flash(Kp, Ki, Kd);
        //     printf("Speed PID parameters saved to flash\n");
        // }
        
        printf("Set speed PID: Kp=%.3f, Ki=%.3f, Kd=%.3f, save=%s\n", 
            Kp, Ki, Kd, save_flag ? "YES" : "NO");
        break;
}

    case FUNC_SET_YAW_PID:
    {
    // 确保数据缓冲区长度足够
    
    
        // 解析PID参数（注意：已放大1000倍，实际值需除以1000）
        uint16_t p_value = (*(data_buf + 5) << 8) | *(data_buf + 4);
        uint16_t i_value = (*(data_buf + 7) << 8) | *(data_buf + 6);
        uint16_t d_value = (*(data_buf + 9) << 8) | *(data_buf + 8);
        uint8_t save_flag = *(data_buf + 10);
        
        // 转换为实际值（除以1000）
        float Kp = (float)p_value / 1000.0f;
        float Ki = (float)i_value / 1000.0f;
        float Kd = (float)d_value / 1000.0f;
        
        // 验证参数范围
        if (p_value > 10000 || i_value > 10000 || d_value > 10000) {
            printf("Error: PID parameters out of range (0-10000)\n");
            break;
        }
        // 设置偏航角PID参数
        PID_Yaw_Set_Parm(Kp, Ki, Kd);

        // 如果需要永久保存参数
        // if (save_flag == 0x5F) {
        //     Save_Yaw_PID_To_Flash(Kp, Ki, Kd);
        //     printf("Yaw PID parameters saved to flash\n");
        // }
        
        printf("Set yaw PID: Kp=%.3f, Ki=%.3f, Kd=%.3f, save=%s\n", 
            Kp, Ki, Kd, save_flag ? "YES" : "NO");
        break;
    }
    case 0x37:  // 开关左车灯（功能码0x37）
    {
  
        
        uint8_t state = *(data_buf + 4);  // 获取状态（0=关，1=开）
        if (state > 1) {  // 验证状态合法性
            printf("Error: Invalid left light state (0-1)\n");
            break;
        }
        
        // 调用现有函数控制左车灯
        if (state == LIGHT_ON) {
            set_on_left_light();
            printf("Left light turned ON\n");
        } else {
            set_off_left_light();
            printf("Left light turned OFF\n");
        }
        break;
    }

    case 0x38:  // 开关右车灯（功能码0x38）
    {
   
        
        uint8_t state = *(data_buf + 4);
        if (state > 1) {
            printf("Error: Invalid right light state (0-1)\n");
            break;
        }
        
        // 调用现有函数控制右车灯
        if (state == LIGHT_ON) {
            set_on_right_light();
            printf("Right light turned ON\n");
        } else {
            set_off_right_light();
            printf("Right light turned OFF\n");
        }
        break;
    }

    case 0x39:  // 开关左右车灯（功能码0x39）
    {
   
        uint8_t state = *(data_buf + 4);
        if (state > 1) {
            printf("Error: Invalid both lights state (0-1)\n");
            break;
        }
        
        // 同时控制左右车灯
        if (state == LIGHT_ON) {
            set_on_left_light();
            set_on_right_light();
            printf("Both lights turned ON\n");
        } else {
            set_off_left_light();
            set_off_right_light();
            printf("Both lights turned OFF\n");
        }
        break;
    }

    case 0x40:  // 开关双闪车灯
    {
        uint8_t state = *(data_buf + 4);
        
        if (state > 1) {
            printf("Error: Invalid hazard light state (0-1)\n");
            break;
        }
        
        if (state == 1) {
            // 开启双闪
            light_hazard_enabled = 1;
            light_last_tick = osKernelGetTickCount();
            printf("Hazard lights turned ON\n");
        } else {
            // 关闭双闪，恢复普通模式
            light_hazard_enabled = 0;
            set_off_left_light();
            set_off_right_light();
            printf("Hazard lights turned OFF\n");
        }
        break;
    }
    case 0x41:  // 原地旋转协议（功能码0x11）
    {

        uint8_t direction = *(data_buf + 4);  // 1=顺时针（左旋转），2=逆时针（右旋转）
        uint16_t angle = *(data_buf + 5) | (*(data_buf + 6) << 8);  // 目标角度（0~360°）
        uint8_t adjust = 0;  // 旋转时关闭偏航角调节（与现有逻辑一致）

        // 验证参数
        if (direction < 1 || direction > 2 || angle > 360) {
            printf("Error: Invalid rotation params\n");
            break;
        }

        // 启动旋转：映射到现有状态
        rotation.enabled = 1;  // 启用旋转闭环
        rotation.direction = direction;
        rotation.target_angle = angle;
        rotation.start_yaw = MPU_Get_Yaw_Now();  // 记录起始角度

        // 调用现有函数启动旋转
        if (direction == 1) {
            // 顺时针旋转 → 左旋转（MOTION_SPIN_LEFT）
            Mecanum_State(MOTION_LEFT, 200, adjust);  // speed=300（可调整）
        } else {
            // 逆时针旋转 → 右旋转（MOTION_SPIN_RIGHT）
            Mecanum_State(MOTION_RIGHT, 200, adjust);
        }
        break;
    }
    case 0x50: {

        uint8_t param_type =*(data_buf+4);  // 参数类型（0x13=电机PID，0x14=偏航角PID）
        uint8_t verify = *(data_buf+5);      // 验证号


        // 根据参数类型返回对应PID参数
        if (param_type == 0x13) {
            // 1. 返回电机PID参数（协议：0xFF 0xFB 0x0A 0x13 验证号 P低 P高 I低 I高 D低 D高 校验和）
            uint8_t response[12] = {0xFF, 0xFB, 0x0A, 0x13, verify};
            
            // 获取电机PID参数（取第一路电机，假设4路参数相同）
            Pid_t *motor_pid = Pid_Get_Motor();
            int32_t kp = (int32_t)(motor_pid[0].Kp * 1000);  // 放大1000倍转为整数（如1.0f → 1000）
            int32_t ki = (int32_t)(motor_pid[0].Ki * 1000);
            int32_t kd = (int32_t)(motor_pid[0].Kd * 1000);

            // 填充参数（低字节在前）
            response[5] = kp & 0xFF;         // P低位
            response[6] = (kp >> 8) & 0xFF;  // P高位
            response[7] = ki & 0xFF;         // I低位
            response[8] = (ki >> 8) & 0xFF;  // I高位
            response[9] = kd & 0xFF;         // D低位
            response[10] = (kd >> 8) & 0xFF; // D高位

            // 计算校验和
            response[11] = CalculateChecksum(response, 11);
            PrintPacket(response, 12);
            
            // 发送应答
            Serial1_SendArray(response, 12);
            printf("Sent motor PID response\n");
        }
        else if (param_type == 0x14) {
            // 2. 返回偏航角PID参数（协议：0xFF 0xFB 0x0A 0x14 验证号 P低 P高 I低 I高 D低 D高 校验和）
            uint8_t response[12] = {0xFF, 0xFB, 0x0A, 0x14, verify};
            
            // 获取偏航角PID参数
            extern PID pid_Yaw;  // 引用全局偏航角PID结构体
            int32_t kp = (int32_t)(pid_Yaw.Proportion * 1000);
            int32_t ki = (int32_t)(pid_Yaw.Integral * 1000);
            int32_t kd = (int32_t)(pid_Yaw.Derivative * 1000);

            // 填充参数（低字节在前）
            response[5] = kp & 0xFF;
            response[6] = (kp >> 8) & 0xFF;
            response[7] = ki & 0xFF;
            response[8] = (ki >> 8) & 0xFF;
            response[9] = kd & 0xFF;
            response[10] = (kd >> 8) & 0xFF;

            // 计算校验和
            response[11] = CalculateChecksum(response, 11);
            PrintPacket(response, 12);

            // 发送应答
            Serial1_SendArray(response, 12);
            printf("Sent yaw PID response\n");
        }
        else {
            printf("PID request: invalid param type 0x%02X\n", param_type);
        }
        break;
    }
    default:
        break;
    }
}
